#include "def.h"
#include "vector_types.h"
#include <stdlib.h>
#include <math.h>
#include <stdio.h>

void kernel_2d_convolution_cpu(uchar4 *p, uchar4 *o, int width, int height, int K ){

  int x,y, i,z,m;

  float a[4],c[4];

  #define c(x,y,d) (p[(y)*width+x].d - p[z].d)
  #define sqr(x) (x*x)
 
  #define N(d) ( y>0 ? c(x,y-1,d) : 0 )
  #define W(d) ( x>0 ? c(x-1,y,d) : 0 )
  #define S(d) ( y<height-1 ? c(x,y+1,d) : 0 )
  #define E(d) ( x<width-1  ? c(x+1,y,d) : 0 )

  #define L(d) a[0]=N(d); a[1]=W(d); a[2]=S(d); a[3]=E(d); 
  for (m=0; m<3; m++) {

    for (x=0; x<width; x++) {
      for (y=0; y<height; y++) {
        z=y*width+x;

        //printf("%d %d %d\n",x,y,z);

        if (m==0) { L(x) } else if (m==1) { L(y) } else { L(z) }
          
        #define Abs(x) (x>0 ? x : -x)
        for (i=0; i<4; i++) {
          c[i] = a[i];//-a[(i+2) % 4];
          c[i] = Abs(c[i])/K;
          //c[i]= 1/(1+A(c[i]));
          c[i]= exp(-sqr(c[i]));
        }

        float r=0;
        for (i=0; i<4; i++) 
          r+=a[i]*c[i];

        #define O(d) o[z].d=p[z].d + r/4;
       // #define O(d) if (p[z].d<150) o[z].d=0; else o[z].d=255;

        if (m==0) { O(x) } else if (m==1) { O(y) } else { O(z) }

        o[z].w=p[z].w;
      }
    }

  }
}
